#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright    :   Jihua Lab 2021.
@File    :   robot_control_node.py
@Author  :   WANG Hao 
@Contact :   wanghao@jihualab.com、haowanghk@gmail.com(Github)
@License :   (C)Copyright 2020-2021, JHL
@Version :   1.3
@Create Time    :  2021-12-20
@Modify Time      @Author    @Version    @Desciption
-------------------        --- -------    ---  --------   --- -----------
@File Description :   DUAL_ARM ros robot control metapackage  
'''
# armParams.py/ all of dual arm servo relative parameters 

CONTROL_MODE = {'TEST_BENCH': 0, 'M600_PLATFORM': 1,}
# different control modes for different control scienials :
# TEST_BENCH': 0   bench testing , mainly the arm init state is diff
# 'M600_PLATFORM': 1

class jointParams(object):
    # 机械臂参数表类：
    #  1. 机械臂   位置控制肩关节：摆动1、俯仰2、肘关节俯仰3运动，
    #             舵机参数：转角范围servoRange、上下限upperBound\lowerBound、初始角度initAngle
    #  2. 腕部姿态及手抓控制  腕部关节滚转 、手抓开合
    #              舵机参数：转角范围servoRange、上下限upperBound\lowerBound、初始角度initAngle
    def __init__(self,control_mode = CONTROL_MODE['M600_PLATFORM']):
        
        self.control_mode = control_mode
        
        LEFT_DEFAULT_ANGLE = \
    [{'Servo_id': 0, 'upper_angle': 168, 'lower_angle': 48, 'zero_angle': 140, 'velocity': 15, 't_acc_dft': 30,
      't_dec_dft': 30, 'power': 8000, 'mean_dps': 100},
     {'Servo_id': 1, 'upper_angle': 85, 'lower_angle': -15, 'zero_angle': 45, 'velocity': 15, 't_acc_dft': 30,
      't_dec_dft': 30,
      'power': 8000, 'mean_dps': 100},
     {'Servo_id': 2, 'upper_angle': 60, 'lower_angle': 165, 'zero_angle': 155, 'velocity': 15, 't_acc_dft': 30,
      't_dec_dft': 30,
      'power': 8000, 'mean_dps': 100}]
# right servo id idex: 4,5,6
# -4.6, -76.5, 160.9
        RIGHT_DEFAULT_ANGLE = \
    [{'Servo_id': 4, 'upper_angle': -32, 'lower_angle': 88, 'zero_angle': -4.6, 'velocity': 15, 't_acc_dft': 30,
      't_dec_dft': 30,
      'power': 8000, 'mean_dps': 100},
     {'Servo_id': 5, 'upper_angle': -110, 'lower_angle': -10, 'zero_angle': -76.5, 'velocity': 15, 't_acc_dft': 30,
      't_dec_dft': 30,
      'power': 8000, 'mean_dps': 100},
     {'Servo_id': 6, 'upper_angle': 68, 'lower_angle': 180, 'zero_angle': 171, 'velocity': 15, 't_acc_dft': 30,
      't_dec_dft': 30,
      'power': 8000, 'mean_dps': 100}]
 

    def get_param(self):
        pass

class wristGripperParams(self):
    def __init__(self,control_mode = CONTROL_MODE['M600_PLATFORM']):

        self.control_mode = control_mode
        self.servo_id_mapping = {'left_wrist': 0, 'left_gripper': 1, 'right_wrist': 2, 'right_gripper': 3}
        self.servo_range_limit = {'left_wrist': 100, 'left_gripper': 95, 'right_wrist': 100, 'right_gripper': 95}
        self.servo_motion_group_init = {'left_wrist': 0, 'left_gripper': 50, 'right_wrist': 0, 'right_gripper': 50}


        if (self.control_mode == 0 ):
            self.servo_zero_init = {'left_wrist': 0, 'left_gripper': 80, 'right_wrist': 0, 'right_gripper': 80}

        else: self.control_mode == 1:
            self.servo_zero_init = {'left_wrist': 0, 'left_gripper': 80, 'right_wrist': 0, 'right_gripper': 80}

